#include "math.h"
#include "User_define.h"
#include "string.h"

//============================//
//============5DOF============//
//============================//

//void MyIMU_Init();
void Gyro_Init();

void MyIMU_get5DOF();
void getInclination5DOF();

void normalize3DVec(float * vector);
float squared(float x);
void Acc_getFinal(float *, int );
void filter(float* , int );
void Gyro_zeroCalibrate(unsigned char , unsigned char );
void getLPCoefficientsButterworth2Pole(const int , const float , float* const , float* const );
/*
float RwAcc[3];		//Projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float Gyro_ds[3];	//Gyro readings
float RwGyro[3];		//Rw obtained from last estimated value and gyro movement
float RwEst[3];
float Awz[2];		//Angles between projection of R on XZ/YZ plane and Z axis (deg)
*/
//int firstSample;
//float deltaT;

//#define wGyro			10.0f
//#define PI				3.141592654f

void SerialPrint(unsigned char c);
void SerialPrintByte(unsigned char b);		// print 1 byte
void SerialFloatPrint(float f);
void SerialIntPrint(int v);
void SerialInt32Print(float f);
void SerialPrintFloatArr(float * arr, unsigned char length);
void SerialPrintlnFloatArr(float * arr, unsigned char length);
void SerialPrintlnIntArr(int * arr, unsigned char length);
void SerialPrintIntArr(int * arr, unsigned char length);
float _8Bytes_To_Float(unsigned char *a);
int _4Bytes_To_Int(unsigned char *a);
unsigned char _2Bytes_To_Byte(unsigned char *a);
/*
void MyIMU_Init()
{
	//lastUpdate = 0; // Make sure you reset all the field to ZERO
	//now = 0;
//	int firstSample;
//    float deltaT;

//	deltaT = 0.005; // 5ms - 0.005s : better to calc. dynamic deltaT. Need to implement millis()
	firstSample = 1;
*/	
//	GYRO_Init();
//}

void GYRO_Init()
{
    firstSample = true;
	Gyro_zeroCalibrate(64, 5);
}

void Gyro_zeroCalibrate(unsigned char totSamples, unsigned char sampleDelayMS)
{
	float x, y, z;
	float gOffsets[3] ={ 0.0,0.0,0.0};
	unsigned char i = 0 ;
	for (i = 0; i < totSamples; i++)
	{
		//DelayUs(sampleDelayMS);
		MPU6050_getRotation(&x, &y, &z);
		gOffsets[0] = (gOffsets[0] * i + x) / (i + 1);
		gOffsets[1] = (gOffsets[1] * i + y) / (i + 1);
		gOffsets[2] = (gOffsets[2] * i + z) / (i + 1);
	}
}

Gyro_getRotation(float* x, float* y, float* z)
{
	 *x =((((AdcRegs.ADCRESULT2>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD) /( 2.0 * VDD * SCALE_FACTOR) );/// 180 * PI;//theta_dot_3 pitch
	 *y =((((AdcRegs.ADCRESULT1>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) );/// 180 * PI ;//theta_dot_1 roll
	 *z =((((AdcRegs.ADCRESULT3>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) );/// 180 * PI;//theta_dot_2 yaw;
}

void MyIMU_get5DOF() {
    float RwAcc[3];		//Projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
    float Gyro_ds[3];	//Gyro readings
	//MPU6050_getMotion9(&RwAcc[0], &RwAcc[1], &RwAcc[2], &Gyro_ds[0], &Gyro_ds[1], &Gyro_ds[2], &val[6], &val[7], &val[8]);
	Acc_getFinal(RwAcc, 3);
	Gyro_getValues(Gyro_ds, 3);
	getInclination5DOF();
}

void getInclination5DOF() {
	int w = 0;
	float tmpf = 0.0;
	int signRzGyro;
	float RwAcc[3];
	float RwEst[3];	
	float RwGyro[3];
	float Awz[2];
	float Gyro_ds[3];
	
	//now = millis();
	//deltaT = (now - lastUpdate) / 1000.0;	// chuyen sang sec
	//lastUpdate = now;
	
	if (firstSample) { // the NaN check is used to wait for good data from the Arduino
		for(w=0;w<=2;w++) {
			RwEst[w] = RwAcc[w];    //initialize with accelerometer readings
		}
	}
	else{
		//evaluate RwGyro vector
		if(abs(RwEst[2]) < 0.1) {
			//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
			//in this case skip the gyro data and just use previous estimate
			for(w=0;w<=2;w++) {
				RwGyro[w] = RwEst[w];
			}
		}
		else {
			//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
			for(w=0;w<=1;w++){
				tmpf = Gyro_ds[w];                      //get current gyro rate in deg/s
				tmpf *= deltaT;							//get angle change in deg
				Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI;   //get angle and convert to degrees
				Awz[w] += tmpf;             //get updated angle according to gyro movement
			}
			
			//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
			//RzGyro is pozitive if  Axz in range -90 ..90 => cos(Awz) >= 0
			signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
			
			//reverse calculation of RwGyro from Awz angles, for formulas deductions see  http://starlino.com/imu_guide.html
			for(w=0;w<=1;w++){
				RwGyro[0] = sin(Awz[0] * PI / 180);
				RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
				RwGyro[1] = sin(Awz[1] * PI / 180);
				RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
			}
			RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
		}
		
		//combine Accelerometer and gyro readings
		for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);

		normalize3DVec(RwEst);
	}
	
	firstSample = 0;
}

void normalize3DVec(float * vector) {
	float R = 0.0;
	R =  sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
	vector[0] /= R;
	vector[1] /= R;
	vector[2] /= R;

}


float squared(float x){
	return x*x;
}


/////----------------------------------////
void Acc_Filter_Value(float *samples, int count)
{
	float Acc[3]={0.0,0.0,0.0};
	int i;
	Acc[0] = (((AdcRegs.ADCRESULT4>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_X_AXIS)*5;
	Acc[1] = (((AdcRegs.ADCRESULT6>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Y_AXIS)*5;
	Acc[2] = (((AdcRegs.ADCRESULT5>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Z_AXIS)*5;
	filter(Acc,3);

	for( i=0;i<count;i++){
	samples[i]=Acc[i];}

}

void Acc_Values_Nomalize(float *Accelerometer, int count)
{
	float Acc_N[3]={0.0,0.0,0.0};
	float norm=0.0;
	int i;
	Acc_Filter_Value(Acc_N,3);
	norm=sqrt(Acc_N[0]*Acc_N[0]+Acc_N[1]*Acc_N[1]+Acc_N[2]*Acc_N[2]);
	Acc_N[0]=Acc_N[0]/norm;
	Acc_N[1]=Acc_N[1]/norm;
	Acc_N[2]=Acc_N[2]/norm;
	for( i=0 ;i<count; i++){
	Accelerometer[i]= Acc_N[i];  }

}

void Acc_getFinal(float *Acc_angle, int count)
{
	float AccAngle[2]={0.0,0.0};
	float Acc_Final[3]={0.0, 0.0, 0.0};
	int i;
	Acc_Values_Nomalize(Acc_Final, 3);
	for( i= 0; i<count; i++)
	{
		AccAngle[0]=asin(-Acc_Final[0]);
		AccAngle[1]=asin(Acc_Final[1]/AccAngle[0]);
	}

}




void filter(float* samples, int count)
{
    float xv[3];
    float yv[3];
	float ax[3];
	float by[3];
	int i;

	getLPCoefficientsButterworth2Pole(100, 80, ax, by);

	for ( i=0;i<count;i++)
	{
		xv[2] = xv[1]; xv[1] = xv[0];
		xv[0] = samples[i];
		yv[2] = yv[1]; yv[1] = yv[0];

		yv[0] =   (ax[0] * xv[0] + ax[1] * xv[1] + ax[2] * xv[2]
		- by[1] * yv[0]
		- by[2] * yv[1]);

		samples[i] = yv[0];
	}
}

void getLPCoefficientsButterworth2Pole(const int samplerate, const float cutoff, float* const ax, float* const by)
{
	//const double PI      = 3.1415926535897932385;
	//const double sqrt2 = 1.4142135623730950488;

	float QcRaw  = (2 * /*PI */3.1415926535897932385 * cutoff) / samplerate; // Find cutoff frequency in [0..PI]
	float QcWarp = tan(QcRaw); // Warp cutoff frequency

	float gain = 1 / (1+/*sqrt2*/1.4142135623730950488/QcWarp + 2/(QcWarp*QcWarp));
	by[2] = (1 - /*sqrt2*/ 1.4142135623730950488/QcWarp + 2/(QcWarp*QcWarp)) * gain;
	by[1] = (2 - 2 * 2/(QcWarp*QcWarp)) * gain;
	by[0] = 1;
	ax[0] = 1 * gain;
	ax[1] = 2 * gain;
	ax[2] = 1 * gain;
}



/*******************************************************************
                                GyroScope
********************************************************************/
/*
void Gyro_getValues(float *samples, int count)
{
	float Gyro[3]={0.0,0.0,0.0};
	int j,i;
	float gOffsets1=0.0;
	float gOffsets2=0.0;
	float gOffsets3=0.0;
	Gyro[0] =((((AdcRegs.ADCRESULT2>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD) /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_3 pitch
	Gyro[1] =((((AdcRegs.ADCRESULT1>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_1 roll
	Gyro[2] =((((AdcRegs.ADCRESULT3>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_2 yaw;
	

	for( j=0;j<64;j++)
	{

		gOffsets1=(gOffsets1*j+Gyro[0])/(j+1);
		gOffsets2=(gOffsets2*j+Gyro[1])/(j+1);
		gOffsets3=(gOffsets3*j+Gyro[2])/(j+1);
	}

	Gyro[0] -= gOffsets1;//pitch
	Gyro[1] -= gOffsets2;//roll
	Gyro[2] -= gOffsets3;//yaw

	for( i=0;i<count;i++)
	samples[i]=Gyro[i];

}
*/
//=========================PRINTF============================//
/*
void SerialPrint(unsigned char c)
{
	while ((UCSR0A & DATA_REGISTER_EMPTY) == 0);
	UDR0 = c;
	asm volatile("nop");//	_delay_ms(10);
}
*/void SerialPrintByte(unsigned char b)		// print 1 byte
{
	unsigned char b1 = (b >> 4) & 0x0f;
	unsigned char b2 = (b & 0x0f);
	unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
	unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
	SerialPrint(c1);
	SerialPrint(c2);
}

void SerialFloatPrint(float f)
{
    unsigned char i=0;
	unsigned char * b = (unsigned char *) &f;
	for( i = 0; i < 4; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialIntPrint(int w)
{
    unsigned char i=0;
	unsigned char * b = (unsigned char *) &w;
	for( i = 0; i < 2; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialInt32Print(float f)
{
    unsigned char i =0 ;
	unsigned char * b = (unsigned char *) &f;
	for(i = 0; i < 4; i++)
	{
		unsigned char b1 = (b[i] >> 4) & 0x0f;
		unsigned char b2 = (b[i] & 0x0f);
		unsigned char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
		unsigned char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
		SerialPrint(c1);
		SerialPrint(c2);
	}
}

void SerialPrintlnFloatArr(float * arr, unsigned char length)
{
    unsigned char i = 0;
	for(i = 0; i < length; i++)
	{
		SerialFloatPrint(arr[i]);
		SerialPrint(',');
	}
//	SerialPrint('\n');
}

void SerialPrintFloatArr(float * arr, unsigned char length)
{
    unsigned char i = 0;
	for( i = 0; i < length; i++)
	{
		SerialFloatPrint(arr[i]);
//		SerialPrint(',');
	}
}

void SerialPrintIntArr(int * arr, unsigned char length)
{
    unsigned char i = 0;
	for( i = 0; i < length; i++)
	{
		SerialIntPrint(arr[i]);
		SerialPrint(',');
	}
//	SerialPrint('\n');
}

float _8Bytes_To_Float(unsigned char *a)
{
	unsigned char b[4];
	unsigned char i = 0;
	float val;
	for ( i = 0; i < 4; i++)
	{
		unsigned char h = (a[i * 2] < 0x3A)? a[i * 2] - 0x30 : a[i * 2] - 0x37;
		unsigned char l = (a[i * 2 + 1] < 0x3A)? a[i * 2 + 1] - 0x30 : a[i * 2 + 1] - 0x37;
		b[i] = h * 16 + l;
	}
	
	memcpy(&val, b, sizeof val);
	return val;
}

int _4Bytes_To_Int(unsigned char *a)
{
	unsigned char b[2];
	unsigned char i =0;
	int val;
	for ( i = 0; i < 2; i++)
	{
		unsigned char h = (a[i * 2] < 0x3A)? a[i * 2] - 0x30 : a[i * 2] - 0x37;
		unsigned char l = (a[i * 2 + 1] < 0x3A)? a[i * 2 + 1] - 0x30 : a[i * 2 + 1] - 0x37;
		b[i] = h * 16 + l;
	}
	
	memcpy(&val, b, sizeof val);
	return val;
}

unsigned char _2Bytes_To_Byte(unsigned char *a)
{
	unsigned char h = (a[0] < 0x3A)? a[0] - 0x30 : a[0] - 0x37;
	unsigned char l = (a[1] < 0x3A)? a[1] - 0x30 : a[1] - 0x37;
	return h * 16 + l;
}

/*float FourBytes_To_Float(unsigned char *_4bytes)
{
	float a = (float) _4bytes[0];
	return a;
}
*/
